/****************************************************************************
 *
 *   Copyright (C) 2024 MicoAir Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file test_intrusive_queue.c
 * @brief Unit tests for intrusive queue
 */

#include <px4_platform_common/intrusive_queue.h>
#include <stddef.h>
#include <string.h>

/* External debug printf function */
extern void debug_printf(const char *format, ...);

/* Test result counters */
static int tests_passed = 0;
static int tests_failed = 0;

/* Test assertion macro */
#define TEST_ASSERT(condition, message) \
    do { \
        if (condition) { \
            debug_printf("[PASS] %s\r\n", message); \
            tests_passed++; \
        } else { \
            debug_printf("[FAIL] %s\r\n", message); \
            tests_failed++; \
        } \
    } while (0)

/**
 * @brief Test data structure
 */
typedef struct {
    int value;
    intrusive_queue_node_t node;
} test_item_t;

/**
 * @brief Test 1: Queue initialization and empty check
 */
static void test_queue_init(void)
{
    debug_printf("\r\n=== Test: Queue Init and Empty ===\r\n");
    
    intrusive_queue_t queue;
    intrusive_queue_init(&queue, offsetof(test_item_t, node));
    
    TEST_ASSERT(intrusive_queue_empty(&queue), "Queue is empty after init");
    TEST_ASSERT(intrusive_queue_front(&queue) == NULL, "Front is NULL when empty");
    TEST_ASSERT(intrusive_queue_back(&queue) == NULL, "Back is NULL when empty");
    TEST_ASSERT(intrusive_queue_size(&queue) == 0, "Size is 0 when empty");
}

/**
 * @brief Test 2: Push and pop operations
 */
static void test_push_pop(void)
{
    debug_printf("\r\n=== Test: Push and Pop ===\r\n");
    
    intrusive_queue_t queue;
    intrusive_queue_init(&queue, offsetof(test_item_t, node));
    
    test_item_t item1 = {.value = 1, .node = {NULL}};
    test_item_t item2 = {.value = 2, .node = {NULL}};
    test_item_t item3 = {.value = 3, .node = {NULL}};
    
    /* Push items */
    intrusive_queue_push(&queue, &item1);
    TEST_ASSERT(!intrusive_queue_empty(&queue), "Queue not empty after push");
    TEST_ASSERT(intrusive_queue_size(&queue) == 1, "Size is 1 after one push");
    TEST_ASSERT(intrusive_queue_front(&queue) == &item1, "Front is item1");
    TEST_ASSERT(intrusive_queue_back(&queue) == &item1, "Back is item1");
    
    intrusive_queue_push(&queue, &item2);
    TEST_ASSERT(intrusive_queue_size(&queue) == 2, "Size is 2 after two pushes");
    TEST_ASSERT(intrusive_queue_front(&queue) == &item1, "Front is still item1");
    TEST_ASSERT(intrusive_queue_back(&queue) == &item2, "Back is item2");
    
    intrusive_queue_push(&queue, &item3);
    TEST_ASSERT(intrusive_queue_size(&queue) == 3, "Size is 3 after three pushes");
    
    /* Pop items (FIFO order) */
    test_item_t *popped1 = (test_item_t *)intrusive_queue_pop(&queue);
    TEST_ASSERT(popped1 == &item1, "First pop returns item1");
    TEST_ASSERT(popped1->value == 1, "Popped item1 has correct value");
    TEST_ASSERT(intrusive_queue_size(&queue) == 2, "Size is 2 after one pop");
    
    test_item_t *popped2 = (test_item_t *)intrusive_queue_pop(&queue);
    TEST_ASSERT(popped2 == &item2, "Second pop returns item2");
    TEST_ASSERT(popped2->value == 2, "Popped item2 has correct value");
    
    test_item_t *popped3 = (test_item_t *)intrusive_queue_pop(&queue);
    TEST_ASSERT(popped3 == &item3, "Third pop returns item3");
    TEST_ASSERT(popped3->value == 3, "Popped item3 has correct value");
    
    TEST_ASSERT(intrusive_queue_empty(&queue), "Queue is empty after all pops");
    TEST_ASSERT(intrusive_queue_pop(&queue) == NULL, "Pop from empty queue returns NULL");
}

/**
 * @brief Test 3: Remove operation
 */
static void test_remove(void)
{
    debug_printf("\r\n=== Test: Remove ===\r\n");
    
    intrusive_queue_t queue;
    intrusive_queue_init(&queue, offsetof(test_item_t, node));
    
    test_item_t item1 = {.value = 1, .node = {NULL}};
    test_item_t item2 = {.value = 2, .node = {NULL}};
    test_item_t item3 = {.value = 3, .node = {NULL}};
    
    intrusive_queue_push(&queue, &item1);
    intrusive_queue_push(&queue, &item2);
    intrusive_queue_push(&queue, &item3);
    
    /* Remove middle item */
    bool removed = intrusive_queue_remove(&queue, &item2);
    TEST_ASSERT(removed, "Remove item2 returns true");
    TEST_ASSERT(intrusive_queue_size(&queue) == 2, "Size is 2 after remove");
    TEST_ASSERT(intrusive_queue_front(&queue) == &item1, "Front is still item1");
    TEST_ASSERT(intrusive_queue_back(&queue) == &item3, "Back is item3");
}

/**
 * @brief Run all intrusive queue unit tests
 */
void intrusive_queue_run_tests(void)
{
    debug_printf("\r\n========================================\r\n");
    debug_printf("  PX4 Intrusive Queue Unit Tests\r\n");
    debug_printf("========================================\r\n");
    
    tests_passed = 0;
    tests_failed = 0;
    
    test_queue_init();
    test_push_pop();
    test_remove();
    
    debug_printf("\r\n========================================\r\n");
    debug_printf("  Test Results\r\n");
    debug_printf("========================================\r\n");
    debug_printf("  Passed: %d\r\n", tests_passed);
    debug_printf("  Failed: %d\r\n", tests_failed);
    debug_printf("  Total:  %d\r\n", tests_passed + tests_failed);
    debug_printf("========================================\r\n\r\n");
}

